/*
 * File - ros_video_publishers_base.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

/** 
 * \cond INTERNAL
 */

#include "config.h"
#include <nodelet/nodelet.h>
#include <sensor_msgs/distortion_models.h>
#include <opencv2/opencv.hpp>

/**
 * \endcond
 */

#include "ros_video_publishers_base.h"

#include "inudev_ros_nodelet.h"

/**
 * \file ros_video_publishers_base.cpp
 *
 * \brief CRosVideoPublishersBase
 *
 * This is the base class for all publishers sharing the video sensors (depth, disparity, video)
 *
 */

/**
 * \cond INTERNAL
 */

static const char rcsid[]="$Id: 2b174cdd18f5f1dc7278e15b26c9333fe005b469 $";

/**
 * \endcond
 */

namespace __INUROS__NAMESPACE__
{
    CRosVideoPublishersBase::CRosVideoPublishersBase(ros::NodeHandle& _node,  std::shared_ptr<image_transport::ImageTransport> _image_transport, std::string _str, CRosSensor* _rosSensor)
        : CRosPublisher(_node, _image_transport, _str, _rosSensor)
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
    }

    void CRosVideoPublishersBase::fillCameraInfoFromVideo(unsigned int frameIndex, int width, int height, int bRightImage)
    {
        std::shared_ptr<InuDev::CCalibrationData> cd = sensor->GetCalibrationData(mCurrentTemprature);

        //mCi.header.frame_id  = getName();
        ros::param::param<std::string>("Device_ID", mVideoFrameID, "");
        mCi.header.frame_id = "camera_link"+mVideoFrameID;        
        mCi.header.seq       = frameIndex;
        mCi.width            = width;
        mCi.height           = height;

        mCi.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;

        if (!mSensors.empty())
        {
            auto c = cd->Sensors[mSensors.at(0)].VirtualCamera.Intrinsic;

            if (!bRightImage)
            {
                c = cd->Sensors[mSensors.at(1)].VirtualCamera.Intrinsic;
            }

            mCi.K[0] = mCi.P[0] = c.FocalLength[0];
            mCi.K[1] = mCi.P[1] = 0.0f;
            mCi.K[2] = mCi.P[2] = c.OpticalCenter[0];
            mCi.K[3] = mCi.P[3] = 0.0f;
            mCi.K[4] = c.FocalLength[1];
            mCi.P[4] = 0;
            mCi.P[5] = c.FocalLength[1];
            mCi.K[5] = mCi.P[6] = c.OpticalCenter[1];
            mCi.K[6] = mCi.P[7] = 0.0f;
            mCi.K[7] = mCi.P[8] = 0.0f;
            mCi.K[8] = 1.0f;
            mCi.P[9] = 0.0f;
            mCi.P[10] = 1.0f;
            mCi.P[11] = 0.0f;
            mCi.R[0] = mCi.R[4] = mCi.R[8] = 1.0f;
            mCi.R[1] = mCi.R[2] = mCi.R[3] = mCi.R[5] = mCi.R[6] = mCi.R[7] = 0.0f;

            // distortion parameters
            mCi.D.resize(c.LensDistortion.size());

            for (std::vector<double>::size_type i=0; i<mCi.D.size(); i++)
            {
                mCi.D.at(i) = c.LensDistortion.at(i);
            }

            if (!bRightImage)
            {
                auto m = cd->Baselines.begin();

                mCi.P[ 3] = m->second;
            }
        }
    }
}
